#ifndef WHEEL_FACTOR_H_
#define WHEEL_FACTOR_H_
#include <iostream>
#include <vector>
#include <memory>

#include "wheel_preintegration.h"
#include "state.h"

class WheelFactor {
public:  
   WheelFactor(std::shared_ptr<WheelPreIntegration> wheel_preintegration);
   void ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 3, 1> &residual, Eigen::Matrix<double, 3, 8> &jacobianXi, Eigen::Matrix<double, 3, 5> &jacobianXj);

   Eigen::Matrix<double, 3, 3> GetCov() {
      return wheel_preintegration_->cov_;
   }

private:
   std::shared_ptr<WheelPreIntegration> wheel_preintegration_;
};

#endif 
